#include "Tube.h"
#include "../Robotmap.h"
#include "../Commands/TubeIdle.h"

Tube::Tube() : Subsystem("Tube") {
	
	brushMotor = new Jaguar(RobotMap::OTHER_BRUSH_MOTOR);
	limit1 = new DigitalInput(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::FEED_BALL_1_SENS);
	limit2 = new DigitalInput(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::FEED_BALL_2_SENS);
	limit3 = new DigitalInput(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::FEED_BALL_3_SENS);
	ballLimit1 = 0;
	ballLimit2 = 0;
	ballLimit3 = 0;
}

void Tube::InitDefaultCommand()
{
	SetDefaultCommand(new TubeIdle());
}


void Tube::doNothing() {
	
}

int Tube::getAmountOfBalls() {
	updateBalls();
	return ballLimit1 + ballLimit2 + ballLimit3;
}


void Tube::setBrushOff() {
	brushMotor->SetSpeed(0.0);
}


void Tube::setBrushOn() {
	brushMotor->SetSpeed(0.8);
}


void Tube::updateBalls() {
	if(limit1->Get())
		ballLimit1 = 1;
	else
		ballLimit1 = 0;
	
	if(limit2->Get())
		ballLimit2 = 1;
	else
		ballLimit2 = 0;
	
	if(limit3->Get())
		ballLimit3 = 1;
	else
		ballLimit3 = 0;
}

bool Tube::getBallSensorStatus(int ballSensorId) {
	switch(ballSensorId) {
	case RobotMap::FEED_BALL_1_SENS:
		return limit1->Get();
	case RobotMap::FEED_BALL_2_SENS:
		return limit2->Get();
	case RobotMap::FEED_BALL_3_SENS:
		return limit3->Get();
	default:
		return false;
	}
}
